//
// Created by Xiezongxi on 2025/7/1.
//
#include "usr_inc.h"

float DribbleGamePoint_X[8] = {0, 0.8,2.5 ,2.5 ,2.5,0.8,0.0,0.0 };
float DribbleGamePoint_Y[8] = {0,-0.8,-0.8,1.25,3.3,3.3,2.5,-1.75};

void StartYunqiusai(void *argument)
{
    uint8_t cmd_temp = 100;       //随便初始化一个用不到的值
    PoseMsg_t YunqiuSaiPose = {0};//希望相对移动的坐标
    PoseMsg_t YunqiuSaiTarPose = {0};//处理后绝对移动的坐标

    // vTaskSuspend(NULL);
    // vTaskDelete(NULL);

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 5; ;)
    {
        YunqiuSai_Command_t cmd;
        xQueueReceive(YunqiuSaiCmdQueueHandle, &cmd, portMAX_DELAY);

        LocaterMsg_t locMsg;
        xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);

        if (cmd == POINT_STOP)
        {
            // cmd_temp = CHASSIS_LOCK;
            // xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
            // vTaskDelay(pdMS_TO_TICKS(100));
            cmd_temp = DRIBBLE_FREE;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(300));
        }
        else if (cmd == POINT_STRAT_YUNQIUSAI) //这个每次进入到运球赛的串口屏界面都会先进到这个cmd中
        {
            YunqiuSaiTarPose.posX = locMsg.posX;
            YunqiuSaiTarPose.posY = locMsg.posY;
            YunqiuSaiTarPose.yaw  = locMsg.posYaw;
        }
        else if (cmd == POINT_DUIKANGSAI)
        {
            cmd_temp = DRIBBLE_EXTEND;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            cmd_temp = DRIBBLE_DRAW;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(2500));
            cmd_temp = CHASSIS_LOCK;
            xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1100));
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1100));
            cmd_temp = DRIBBLE_RETRACT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(400));
            cmd_temp = DRIBBLE_FREE;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);

            cmd_temp = CHASSIS_FREE;
            xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(50));
            cmd_temp = CHASSIS_CONF_JOY;
            xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
        }
        else if (cmd == POINT_DRIBBLE_TEST)
        {
            cmd_temp = DRIBBLE_HOLD;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(250));
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1000));
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1100));
        }
        else if (cmd == POINT_1)
        {
            cmd_temp = DRIBBLE_EXTEND;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            cmd_temp = DRIBBLE_DRAW;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(2000));
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1400));
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1400));

            cmd_temp = CHASSIS_CONF_POINT;
            xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(200));
            YunqiuSaiPose.posX =  0.5;
            YunqiuSaiPose.posY = -0.5;
            // YunqiuSaiTarPose.posX += YunqiuSaiPose.posX * cosf(locMsg.posYaw / 180.0 * M_PI) + YunqiuSaiPose.posY * sinf(locMsg.posYaw / 180.0 * M_PI);
            // YunqiuSaiTarPose.posY += (-1) * YunqiuSaiPose.posX * sinf(locMsg.posYaw / 180.0 * M_PI) + YunqiuSaiPose.posY * cosf(locMsg.posYaw / 180.0 * M_PI);
            YunqiuSaiTarPose.posX += YunqiuSaiPose.posX * cosf(YunqiuSaiTarPose.yaw / 180.0 * M_PI) - YunqiuSaiPose.posY * sinf(YunqiuSaiTarPose.yaw / 180.0 * M_PI);
            YunqiuSaiTarPose.posY += YunqiuSaiPose.posX * sinf(YunqiuSaiTarPose.yaw / 180.0 * M_PI) + YunqiuSaiPose.posY * cosf(YunqiuSaiTarPose.yaw / 180.0 * M_PI);
            xQueueSendToBack(chassisPoseCmdQueueHandle, &YunqiuSaiTarPose, 1);
        }
        else if (cmd == POINT_2)
        {
            cmd_temp = DRIBBLE_DRAW;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1000));
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1000));
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1000));

            // cmd_temp = CHASSIS_CONF_POINT;
            // xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
            // vTaskDelay(pdMS_TO_TICKS(100));
            YunqiuSaiPose.posX = 0.5;
            YunqiuSaiPose.posY = 0;
            YunqiuSaiTarPose.posX += YunqiuSaiPose.posX * cosf(YunqiuSaiTarPose.yaw / 180.0 * M_PI) - YunqiuSaiPose.posY * sinf(YunqiuSaiTarPose.yaw / 180.0 * M_PI);
            YunqiuSaiTarPose.posY += YunqiuSaiPose.posX * sinf(YunqiuSaiTarPose.yaw / 180.0 * M_PI) + YunqiuSaiPose.posY * cosf(YunqiuSaiTarPose.yaw / 180.0 * M_PI);
            xQueueSendToBack(chassisPoseCmdQueueHandle, &YunqiuSaiTarPose, 1);
        }
        else if (cmd == POINT_3)
        {
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1000));
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1000));

            YunqiuSaiPose.posX = 0;
            YunqiuSaiPose.posY = 1.5;
            YunqiuSaiTarPose.posX += YunqiuSaiPose.posX * cosf(YunqiuSaiTarPose.yaw / 180.0 * M_PI) - YunqiuSaiPose.posY * sinf(YunqiuSaiTarPose.yaw / 180.0 * M_PI);
            YunqiuSaiTarPose.posY += YunqiuSaiPose.posX * sinf(YunqiuSaiTarPose.yaw / 180.0 * M_PI) + YunqiuSaiPose.posY * cosf(YunqiuSaiTarPose.yaw / 180.0 * M_PI);

            xQueueSendToBack(chassisPoseCmdQueueHandle, &YunqiuSaiTarPose, 1);
        }
        else if (cmd == POINT_4)
        {
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1000));
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1000));

            YunqiuSaiPose.posX = 0;
            YunqiuSaiPose.posY = 1.5;
            YunqiuSaiTarPose.posX += YunqiuSaiPose.posX * cosf(YunqiuSaiTarPose.yaw / 180.0 * M_PI) - YunqiuSaiPose.posY * sinf(YunqiuSaiTarPose.yaw / 180.0 * M_PI);
            YunqiuSaiTarPose.posY += YunqiuSaiPose.posX * sinf(YunqiuSaiTarPose.yaw / 180.0 * M_PI) + YunqiuSaiPose.posY * cosf(YunqiuSaiTarPose.yaw / 180.0 * M_PI);
            xQueueSendToBack(chassisPoseCmdQueueHandle, &YunqiuSaiTarPose, 1);
        }
        else if (cmd == POINT_5)
        {

            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1000));
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1000));

            YunqiuSaiPose.posX = -1;
            YunqiuSaiPose.posY =  0;
            YunqiuSaiTarPose.posX += YunqiuSaiPose.posX * cosf(YunqiuSaiTarPose.yaw / 180.0 * M_PI) - YunqiuSaiPose.posY * sinf(YunqiuSaiTarPose.yaw / 180.0 * M_PI);
            YunqiuSaiTarPose.posY += YunqiuSaiPose.posX * sinf(YunqiuSaiTarPose.yaw / 180.0 * M_PI) + YunqiuSaiPose.posY * cosf(YunqiuSaiTarPose.yaw / 180.0 * M_PI);
            xQueueSendToBack(chassisPoseCmdQueueHandle, &YunqiuSaiTarPose, 1);
        }
        else if (cmd == POINT_6)
        {
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1000));
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1000));

            cmd_temp = CHASSIS_CONF_POINT;
            xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(100));
            YunqiuSaiPose.posX = -0.5;
            YunqiuSaiPose.posY =  0.5;
            YunqiuSaiTarPose.posX += YunqiuSaiPose.posX * cosf(YunqiuSaiTarPose.yaw / 180.0 * M_PI) - YunqiuSaiPose.posY * sinf(YunqiuSaiTarPose.yaw / 180.0 * M_PI);
            YunqiuSaiTarPose.posY += YunqiuSaiPose.posX * sinf(YunqiuSaiTarPose.yaw / 180.0 * M_PI) + YunqiuSaiPose.posY * cosf(YunqiuSaiTarPose.yaw / 180.0 * M_PI);

            xQueueSendToBack(chassisPoseCmdQueueHandle, &YunqiuSaiTarPose, 1);
        }
        else if (cmd == POINT_7)
        {
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1000));
            cmd_temp = DRIBBLE_PAT;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(1000));

            cmd_temp = CHASSIS_CONF_POINT;
            xQueueSendToBack(chassisCmdQueueHandle, &cmd, 1);
            vTaskDelay(pdMS_TO_TICKS(100));
            YunqiuSaiPose.posX = 0;
            YunqiuSaiPose.posY = -2.5;
            YunqiuSaiTarPose.posX += YunqiuSaiPose.posX * cosf(YunqiuSaiTarPose.yaw / 180.0 * M_PI) - YunqiuSaiPose.posY * sinf(YunqiuSaiTarPose.yaw / 180.0 * M_PI);
            YunqiuSaiTarPose.posY += YunqiuSaiPose.posX * sinf(YunqiuSaiTarPose.yaw / 180.0 * M_PI) + YunqiuSaiPose.posY * cosf(YunqiuSaiTarPose.yaw / 180.0 * M_PI);

            xQueueSendToBack(chassisPoseCmdQueueHandle, &YunqiuSaiTarPose, 1);
        }

        // vTaskDelay(pdMS_TO_TICKS(period));
        // printf("%d,%f,%f\r\n",cmd,YunqiuSaiTarPose.posX,YunqiuSaiTarPose.posY);
    }
}

uint8_t Current_Point_Flag = 100;
uint8_t GoBack_Point_Flag ;
static uint8_t Circle_Flag = 0;
static uint8_t Only_Dribble_Twice_Flag = 0;
DribbleGame_Command_t dribblegameCmd;
void StartDribbleGame(void *argument)
{
    uint8_t cmd_temp = 100;       //随便初始化一个用不到的值
    PoseMsg_t YunqiuSaiPose = {0};//希望相对移动的坐标
    PoseMsg_t YunqiuSaiTarPose = {0};//处理后绝对移动的坐标

    vTaskDelete(NULL);

    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 10; ;)
    {
        // DribbleGame_Command_t dribblegameCmd;
        // xQueueReceive(DribbleGameCmdQueueHandle, &dribblegameCmd, 1);
        // xQueueReceive(DribbleGameCmdQueueHandle, &dribbleCmd, portMAX_DELAY);
        LocaterMsg_t locMsg;
        xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);

        if (dribblegameCmd == AUTO)
        {
            /* 检测当前的位置 */
            if(fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[0]) <= 0.08 && fabs(locMsg.posY-START_POINT_Y-DribbleGamePoint_Y[0]) <= 0.08)
            {
                if(Circle_Flag>0)
                {
                    cmd_temp = STOP;
                    xQueueSendToBack(DribbleGameCmdQueueHandle, &cmd_temp, 1);
                    vTaskSuspend(NULL);
                }
                else
                {
                    Current_Point_Flag = 0;
                }
            }
            else if(fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[1]) <= 0.03 && fabs(locMsg.posY-START_POINT_Y-DribbleGamePoint_Y[1]) <= 0.03)
            {
                Current_Point_Flag = 1;
                Circle_Flag++;
            }
            else if(fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[2]) <= 0.03 && fabs(locMsg.posY-START_POINT_Y-DribbleGamePoint_Y[2]) <= 0.03)
            {
                Current_Point_Flag = 2;
                Circle_Flag++;
            }
            else if(fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[3]) <= 0.05 && fabs(locMsg.posY-START_POINT_Y-DribbleGamePoint_Y[3]) <= 0.05)
            {
                Current_Point_Flag = 3;
                Circle_Flag++;
            }
            else if(fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[4]) <= 0.05 && fabs(locMsg.posY-START_POINT_X-DribbleGamePoint_Y[4]) <= 0.05)
            {
                Current_Point_Flag = 4;
                Circle_Flag++;
            }
            else if(fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[5]) <= 0.05 && fabs(locMsg.posY-START_POINT_Y-DribbleGamePoint_Y[5]) <= 0.05)
            {
                Current_Point_Flag = 5;
                Circle_Flag++;
            }
            else if(fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[6]) <= 0.05 && fabs(locMsg.posY-START_POINT_Y-DribbleGamePoint_Y[6]) <= 0.05)
            {
                Current_Point_Flag = 6;
                Circle_Flag++;
            }
            else
            {
                Current_Point_Flag = 100;
                // Circle_Flag++;
            }

            if (Current_Point_Flag == 0)
            {
                GoBack_Point_Flag = 0;
                // if(Circle_Flag>0)
                // {
                //     cmd_temp = STOP;
                //     xQueueSendToBack(DribbleGameCmdQueueHandle, &cmd_temp, 1);
                // }
                // Current_Point_Flag = 100;
                // else
                // {
                    cmd_temp = DRIBBLE_HOLD;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);

                    cmd_temp = CHASSIS_Right_Front;
                    xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(1500));

                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(1350));
                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(1000));
                // }
            }
            else if (Current_Point_Flag == 1)
            {
                GoBack_Point_Flag = 1;
                cmd_temp = CHASSIS_Front;
                xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1500));

                cmd_temp = DRIBBLE_PAT;
                xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1350));
                cmd_temp = DRIBBLE_PAT;
                xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1000));
            }
            else if (Current_Point_Flag == 2)
            {
                GoBack_Point_Flag = 2;
                cmd_temp = CHASSIS_Left;
                xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1500));

                cmd_temp = DRIBBLE_PAT;
                xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1350));
                cmd_temp = DRIBBLE_PAT;
                xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1000));
            }
            else if (Current_Point_Flag == 3)
            {
                GoBack_Point_Flag = 3;
                cmd_temp = CHASSIS_Left;
                xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1500));

                cmd_temp = DRIBBLE_PAT;
                xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1350));
                cmd_temp = DRIBBLE_PAT;
                xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1000));
            }
            else if (Current_Point_Flag == 4)
            {
                GoBack_Point_Flag = 4;
                cmd_temp = CHASSIS_Back;
                xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1500));

                cmd_temp = DRIBBLE_PAT;
                xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1350));
                cmd_temp = DRIBBLE_PAT;
                xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1000));
            }
            else if (Current_Point_Flag == 5)
            {
                GoBack_Point_Flag = 5;
                cmd_temp = CHASSIS_Right_Back;
                xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1500));

                cmd_temp = DRIBBLE_PAT;
                xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1350));
                cmd_temp = DRIBBLE_PAT;
                xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1000));
            }
            else if (Current_Point_Flag == 6)
            {
                GoBack_Point_Flag = 6;
                cmd_temp = CHASSIS_Right;
                xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1500));

                cmd_temp = DRIBBLE_PAT;
                xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1350));
                cmd_temp = DRIBBLE_PAT;
                xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(1000));
            }
            // printf("%d\r\n",dribblegameCmd);
            // printf("%d,%f,%f,%f,%f\r\n",Current_Point_Flag,locMsg.posX,locMsg.posY,locMsg.velX,locMsg.velY);
        }
        // else
        // {
        //     // vTaskDelay(pdMS_TO_TICKS(5));
        //     vTaskSuspend(NULL);
        // }
        vTaskDelay(pdMS_TO_TICKS(period));
    }
}

void StartDribbleGame2(void *argument)
{
    uint8_t cmd_temp = 100;       //随便初始化一个用不到的值
    PoseMsg_t YunqiuSaiPose = {0};//希望相对移动的坐标
    PoseMsg_t DribbleGameTarPose = {0};//处理后绝对移动的坐标


    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 10; ;)
    {
        // DribbleGame_Command_t dribblegameCmd;
        // xQueueReceive(DribbleGameCmdQueueHandle, &dribblegameCmd, 1);
        // xQueueReceive(DribbleGameCmdQueueHandle, &dribbleCmd, portMAX_DELAY);
        LocaterMsg_t locMsg;
        xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);

        if (dribblegameCmd == AUTO)
        {
            cmd_temp = DRIBBLE_HOLD;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            /* 检测当前的位置 */
            // Circle_Flag = 0;
            /* 退出循环的条件：Current_Point_Flag == 0 且 Circle_Flag > 0 */
            // while((Current_Point_Flag != 0) || (Circle_Flag <= 0))
            if(fabs(locMsg.posX) <= 0.06 && fabs(locMsg.posY) <= 0.06)
            {
                Current_Point_Flag = 0;
                // Only_Dribble_Twice_Flag = 0;
            }
            else if(fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[0]) <= 0.06 && fabs(locMsg.posY-START_POINT_Y-DribbleGamePoint_Y[0]) <= 0.06)
            {
                // if(Circle_Flag>0)
                // {
                //     cmd_temp = STOP;
                //     xQueueSendToBack(DribbleGameCmdQueueHandle, &cmd_temp, 1);
                //     vTaskSuspend(NULL);
                // }
                // else
                // {
                    Current_Point_Flag = 1;
                    Only_Dribble_Twice_Flag = 0;
                    // Circle_Flag++;
                // }
            }
            else if(fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[1]) <= 0.06 && fabs(locMsg.posY-START_POINT_Y-DribbleGamePoint_Y[1]) <= 0.06)
            {
                Current_Point_Flag = 2;
                Only_Dribble_Twice_Flag = 0;
                Circle_Flag++;
            }
            else if(fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[2]) <= 0.06 && fabs(locMsg.posY-START_POINT_Y-DribbleGamePoint_Y[2]) <= 0.06)
            {
                Current_Point_Flag = 3;
                Only_Dribble_Twice_Flag = 0;
                Circle_Flag++;
            }
            else if(fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[3]) <= 0.06 && fabs(locMsg.posY-START_POINT_Y-DribbleGamePoint_Y[3]) <= 0.06)
            {
                Current_Point_Flag = 4;
                Only_Dribble_Twice_Flag = 0;
                Circle_Flag++;
            }
            else if(fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[4]) <= 0.06 && fabs(locMsg.posY-START_POINT_Y-DribbleGamePoint_Y[4]) <= 0.06)
            {
                Current_Point_Flag = 5;
                Only_Dribble_Twice_Flag = 0;
                Circle_Flag++;
            }
            else if(fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[5]) <= 0.06 && fabs(locMsg.posY-START_POINT_Y-DribbleGamePoint_Y[5]) <= 0.06)
            {
                Current_Point_Flag = 6;
                Only_Dribble_Twice_Flag = 0;
                Circle_Flag++;
            }
            else if(fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[6]) <= 0.06 && fabs(locMsg.posY-START_POINT_Y-DribbleGamePoint_Y[6]) <= 0.06)
            {
                Current_Point_Flag = 7;
                Only_Dribble_Twice_Flag = 0;
                Circle_Flag++;
            }
            else if(fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[7]) <= 0.05 && fabs(locMsg.posY-START_POINT_Y-DribbleGamePoint_Y[7]) <= 0.05)
            {
                Current_Point_Flag = 8;
                Only_Dribble_Twice_Flag = 0;
                Circle_Flag++;
            }
            else
            {
                Current_Point_Flag = 100;
                // Only_Dribble_Twice_Flag = 1;
                // Circle_Flag++;
            }
            if (Current_Point_Flag == 0)
            {
                DribbleGameTarPose.posX = START_POINT_X + DribbleGamePoint_X[0];
                DribbleGameTarPose.posY = START_POINT_Y + DribbleGamePoint_Y[0];
                DribbleGameTarPose.yaw = 0;
                xQueueSendToBack(chassisPoseCmdQueueHandle, &DribbleGameTarPose, 1);
                cmd_temp = CHASSIS_CONF_POINT;
                xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
                vTaskDelay(pdMS_TO_TICKS(800));
            }
            else if (Current_Point_Flag == 1)
            {
                if(Only_Dribble_Twice_Flag == 0 && Circle_Flag ==0)
                {
                    Only_Dribble_Twice_Flag = 1;
                    GoBack_Point_Flag = 0;

                    cmd_temp = DRIBBLE_HOLD;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(225));

                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(1000));
                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(500));
                    cmd_temp = CHASSIS_CONF_POINT;
                    xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(100));
                    DribbleGameTarPose.posX = START_POINT_X + DribbleGamePoint_X[1];
                    DribbleGameTarPose.posY = START_POINT_Y + DribbleGamePoint_Y[1];
                    DribbleGameTarPose.yaw = 90;
                    xQueueSendToBack(chassisPoseCmdQueueHandle, &DribbleGameTarPose, 1);
                    vTaskDelay(pdMS_TO_TICKS(800));
                }

                // vTaskDelay(pdMS_TO_TICKS(200));
                // }
            }
            else if (Current_Point_Flag == 2)
            {
                if(Only_Dribble_Twice_Flag == 0)
                {
                    Only_Dribble_Twice_Flag = 1;
                    GoBack_Point_Flag = 1;
                    vTaskDelay(pdMS_TO_TICKS(175));

                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(1000));
                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(500));
                    DribbleGameTarPose.posX = START_POINT_X + DribbleGamePoint_X[2];
                    DribbleGameTarPose.posY = START_POINT_Y + DribbleGamePoint_Y[2];
                    DribbleGameTarPose.yaw = 90;
                    xQueueSendToBack(chassisPoseCmdQueueHandle, &DribbleGameTarPose, 1);

                    vTaskDelay(pdMS_TO_TICKS(800));
                }
            }
            else if (Current_Point_Flag == 3)
            {
                if(Only_Dribble_Twice_Flag == 0)
                {
                    Only_Dribble_Twice_Flag = 1;
                    GoBack_Point_Flag = 2;
                    vTaskDelay(pdMS_TO_TICKS(175));

                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(1000));
                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(500));
                    DribbleGameTarPose.posX = START_POINT_X + DribbleGamePoint_X[3];
                    DribbleGameTarPose.posY = START_POINT_Y + DribbleGamePoint_Y[3];
                    DribbleGameTarPose.yaw = 180;
                    xQueueSendToBack(chassisPoseCmdQueueHandle, &DribbleGameTarPose, 1);

                    vTaskDelay(pdMS_TO_TICKS(800));
                }
            }
            else if (Current_Point_Flag == 4)
            {
                if(Only_Dribble_Twice_Flag == 0)
                {
                    Only_Dribble_Twice_Flag = 1;
                    GoBack_Point_Flag = 3;
                    vTaskDelay(pdMS_TO_TICKS(175));

                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(1000));
                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(500));
                    DribbleGameTarPose.posX = START_POINT_X + DribbleGamePoint_X[4];
                    DribbleGameTarPose.posY = START_POINT_Y + DribbleGamePoint_Y[4];
                    DribbleGameTarPose.yaw = 180;
                    xQueueSendToBack(chassisPoseCmdQueueHandle, &DribbleGameTarPose, 1);

                    vTaskDelay(pdMS_TO_TICKS(800));
                }
            }
            else if (Current_Point_Flag == 5)
            {
                if(Only_Dribble_Twice_Flag == 0)
                {
                    Only_Dribble_Twice_Flag = 1;
                    GoBack_Point_Flag = 4;
                    vTaskDelay(pdMS_TO_TICKS(175));

                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(1000));
                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(500));
                    DribbleGameTarPose.posX = START_POINT_X + DribbleGamePoint_X[5];
                    DribbleGameTarPose.posY = START_POINT_Y + DribbleGamePoint_Y[5];
                    DribbleGameTarPose.yaw = 270;
                    xQueueSendToBack(chassisPoseCmdQueueHandle, &DribbleGameTarPose, 1);

                    vTaskDelay(pdMS_TO_TICKS(800));
                }
            }
            else if (Current_Point_Flag == 6)
            {
                if(Only_Dribble_Twice_Flag == 0)
                {
                    Only_Dribble_Twice_Flag = 1;
                    GoBack_Point_Flag = 5;
                    vTaskDelay(pdMS_TO_TICKS(175));

                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(1000));
                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(500));
                    DribbleGameTarPose.posX = START_POINT_X + DribbleGamePoint_X[6];
                    DribbleGameTarPose.posY = START_POINT_Y + DribbleGamePoint_Y[6];
                    DribbleGameTarPose.yaw = 270;
                    xQueueSendToBack(chassisPoseCmdQueueHandle, &DribbleGameTarPose, 1);

                    vTaskDelay(pdMS_TO_TICKS(800));
                }
            }
            else if (Current_Point_Flag == 7)
            {
                if(Only_Dribble_Twice_Flag == 0)
                {
                    Only_Dribble_Twice_Flag = 1;
                    GoBack_Point_Flag = 6;
                    vTaskDelay(pdMS_TO_TICKS(175));

                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(1000));
                    cmd_temp = DRIBBLE_PAT;
                    xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
                    vTaskDelay(pdMS_TO_TICKS(500));
                    DribbleGameTarPose.posX = START_POINT_X + DribbleGamePoint_X[7];
                    DribbleGameTarPose.posY = START_POINT_Y + DribbleGamePoint_Y[7];
                    DribbleGameTarPose.yaw = 360;
                    xQueueSendToBack(chassisPoseCmdQueueHandle, &DribbleGameTarPose, 1);

                    vTaskDelay(pdMS_TO_TICKS(800));
                }
                else if (Current_Point_Flag == 8)
                {
                    DribbleGameTarPose.posX = START_POINT_X + DribbleGamePoint_X[7];
                    DribbleGameTarPose.posY = START_POINT_Y + DribbleGamePoint_Y[7];
                    DribbleGameTarPose.yaw = 360;
                    xQueueSendToBack(chassisPoseCmdQueueHandle, &DribbleGameTarPose, 1);
                    vTaskDelay(pdMS_TO_TICKS(100));
                    cmd_temp = STOP;
                    xQueueSendToBack(DribbleGameCmdQueueHandle, &cmd_temp, 1);
                }
            }
        }
        vTaskDelay(pdMS_TO_TICKS(period));
    }
}

void StartDribbleGameErr(void *argument)
{
    uint8_t cmd_temp = 100;       //随便初始化一个用不到的值
    PoseMsg_t YunqiuSaiPose = {0};//希望相对移动的坐标
    PoseMsg_t YunqiuSaiTarPose = {0};//处理后绝对移动的坐标
    TickType_t xLastWakeTime = xTaskGetTickCount();
    for (const int16_t period = 10; ;)
    {
        xQueueReceive(DribbleGameCmdQueueHandle, &dribblegameCmd, 1);
        // xQueueReceive(DribbleGameCmdQueueHandle, &dribbleCmd, portMAX_DELAY);

        LocaterMsg_t locMsg;
        xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);

        if(dribblegameCmd == AUTO)
        {
            // vTaskSuspend(DribbleGameHandle);
            vTaskResume(DribbleGame2Handle);
        }
        else if (dribblegameCmd == PREPARE)
        {
            // vTaskSuspend(DribbleGameHandle);
            vTaskSuspend(DribbleGame2Handle);
            cmd_temp = DRIBBLE_EXTEND;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(100));
            cmd_temp = DRIBBLE_DRAW;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            // vTaskResume(DribbleGameHandle);
        }
        else if (dribblegameCmd == TRY_AGAIN)
        {
            /* 走点走到装球点 */
            // vTaskSuspend(DribbleGameHandle);
            vTaskSuspend(DribbleGame2Handle);

            cmd_temp = CHASSIS_CONF_JOY;
            xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(100));
            // PoseMsg_t DribbleTarPose;
            // DribbleTarPose.posX = TRY_AGAIN_POINT_X;
            // DribbleTarPose.posY = TRY_AGAIN_POINT_Y;
            // xQueueSendToBack(chassisPoseCmdQueueHandle, &DribbleTarPose,1);
            cmd_temp = 0;
            xQueueSendToBack(DribbleGameCmdQueueHandle, &cmd_temp, 1);
            cmd_temp = DRIBBLE_FREE;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(100));
            cmd_temp = DRIBBLE_DRAW;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
        }
        else if (dribblegameCmd == GO_BACK)
        {
            /* 走点走到装球点 */
            // vTaskSuspend(DribbleGameHandle);
            vTaskSuspend(DribbleGame2Handle);

            cmd_temp = DRIBBLE_HOLD;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            vTaskDelay(pdMS_TO_TICKS(100));

            cmd_temp = CHASSIS_CONF_POINT;
            xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
            // vTaskDelay(pdMS_TO_TICKS(100));
            PoseMsg_t DribbleTarPose;
            // DribbleTarPose.posX = DribbleGamePoint_X[GoBack_Point_Flag] + START_POINT_X;
            // DribbleTarPose.posY = DribbleGamePoint_Y[GoBack_Point_Flag] + START_POINT_Y;
            // xQueueSendToBack(chassisPoseCmdQueueHandle, &DribbleTarPose,1);
            // vTaskDelay(pdMS_TO_TICKS(100));
            DribbleTarPose.posX = DribbleGamePoint_X[GoBack_Point_Flag] + START_POINT_X;
            DribbleTarPose.posY = DribbleGamePoint_Y[GoBack_Point_Flag] + START_POINT_Y;
            // DribbleTarPose.posY = 0;
            xQueueSendToBack(chassisPoseCmdQueueHandle, &DribbleTarPose,1);
            vTaskDelay(pdMS_TO_TICKS(100));
            while(! (fabs(locMsg.posX-START_POINT_X-DribbleGamePoint_X[GoBack_Point_Flag]) <= 0.06 && fabs(locMsg.posY-START_POINT_Y-DribbleGamePoint_Y[GoBack_Point_Flag]) <= 0.06))
            {
                // dribblegameCmd = AUTO;
                xQueuePeek(locaterMsgBoxHandle, &locMsg, portMAX_DELAY);
            }
            vTaskDelay(pdMS_TO_TICKS(500));
            // cmd_temp = DRIBBLE_FREE;
            // xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            cmd_temp = AUTO;
            xQueueSendToBack(DribbleGameCmdQueueHandle, &cmd_temp, 1);
        }
        else if (dribblegameCmd == STOP)
        {
            // vTaskSuspend(DribbleGameHandle);
            vTaskSuspend(DribbleGame2Handle);

            cmd_temp = CHASSIS_CONF_JOY;
            xQueueSendToBack(chassisCmdQueueHandle, &cmd_temp, 1);
            cmd_temp = DRIBBLE_FREE;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            cmd_temp = DRIBBLE_DRAW;
            xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            // vTaskDelay(pdMS_TO_TICKS(1000));
            // cmd_temp = DRIBBLE_RETRACT;
            // xQueueSendToBack(dribbleCmdQueueHandle, &cmd_temp, 1);
            cmd_temp = 0;
            xQueueSendToBack(DribbleGameCmdQueueHandle, &cmd_temp, 1);
        }
        else if (dribblegameCmd == CIRCLE)
        {
            // vTaskSuspend(DribbleGameHandle);

            Circle_Flag = 0;
            cmd_temp = AUTO;
            xQueueSendToBack(DribbleGameCmdQueueHandle, &cmd_temp, 1);
        }
        // printf("%d\r\n",Current_Point_Flag);
        vTaskDelay(pdMS_TO_TICKS(period));
        // printf("%d\r\n",dribblegameCmd);
    }
}